#pragma once
namespace graph
{
   namespace algorithm
   {      
      template<mode_e mode, directional_e directional>
      astar_search_t<mode, directional>::astar_search_t
         (graph_view_ptr_t view, astar_initializer_t& initializer)
         :  view_(view)
         ,  vertex_data_fwd_(view->get_graph().get_vertex_count())
         ,  vertex_data_bkwd_(view->get_graph().get_vertex_count())
         ,  is_ended_fwd_(false)
         ,  is_ended_bkwd_(false) 
       {
           initializer.init_me(*this);
           //std::cout << "initialized " <<std::endl;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::run(bool iterate, unsigned long long iteration_number)
       {
           if (directional == undirectional)
           {
               run_undirect(iterate, iteration_number);
           }
           else if (directional == bidirectional)
           {
               run_bidirect(iterate, iteration_number);
           }
       }
       template<mode_e mode, directional_e directional>
       vertex_debug_t astar_search_t<mode, directional>::get_debug_vertex_by_id(vertex_id_t id, bool fwd_search)
       {
           vertex_debug_t result;
           vertex_t const & v = view_->get_vertex_by_id(id);
           if (!fwd_search)
           {
               result.distance_ = get_distance_bkwd(id);
           }
           else 
           {
               result.distance_ = get_distance_fwd(id);
           }           
           result.lat_ = v.get_point().lat_;
           result.lon_ = v.get_point().lon_;
           result.id_ = v.get_id();
           return result;
       }
       
       template<mode_e mode, directional_e directional>
       vertex_t const & astar_search_t<mode, directional>::get_vertex_by_id(vertex_id_t id) const
       {
           return view_->get_graph().get_vertex_by_id(id);
       }
       
       template<mode_e mode, directional_e directional>
       template<search_direction_e direction>
       lat_lon_t astar_search_t<mode, directional>::get_potential(point_t p)
       {
           if (directional == undirectional)
               return math::distance( p, finish_p_);
           else if (directional == bidirectional)
           {
               if (direction == forward)
                   return potential_fwd(p);
               else
                   return potential_bkwd(p);
           }
       }
       
       
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_all_ended() const
       {
           if (directional == undirectional)
           {
               return is_ended_fwd_;
           }
           else if (directional == bidirectional)
           {
               return is_ended_fwd_ && is_ended_bkwd_;
           }
       }
       template<mode_e mode, directional_e directional>
       lat_lon_t astar_search_t<mode, directional>::potential_fwd(point_t p)
       {
           //avg function
           return -potential_bkwd(p);
       }
       template<mode_e mode, directional_e directional>
       lat_lon_t astar_search_t<mode, directional>::potential_bkwd(point_t p)
       {        
           //avg func
           lat_lon_t val1 = math::distance(p, start_p_);
           lat_lon_t val2 = math::distance(p, finish_p_);
           return (val1 - val2)/2;        
       }
       
       template<mode_e mode, directional_e directional>
       vertex_id_t astar_search_t<mode, directional>::extract_min_fwd()
       {
           if (mode == debug)
              debug_info_->min_queue_fwd_extract_min_fwd_calls_ += 1;
           return min_queue_fwd_.extract_min();
       }
       template<mode_e mode, directional_e directional>
       vertex_id_t astar_search_t<mode, directional>::extract_min_bkwd()
       {
           return min_queue_bkwd_.extract_min();
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::new_vertex_fwd(vertex_id_t id, 
                                                            lat_lon_t distance, 
                                                            vertex_id_t pred_id)
       {
           if (mode == debug)
           {
               debug_info_->set_predecessor_fwd_calls_ += 1;
               debug_info_->min_queue_fwd_put_calls_ += 1;
               if (min_queue_fwd_.size() > debug_info_->min_queue_fwd_max_elems_)
                   debug_info_->min_queue_fwd_max_elems_ = min_queue_fwd_.size();
           }
           min_queue_fwd_.insert(distance, id);
           astar_vertex_data_t& data = vertex_data_fwd_[id];
           data.color_ = astar_vertex_data_t::VISITED;
           data.distance_ = distance;
           data.predecessor_ = pred_id;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::new_vertex_bkwd(vertex_id_t id, 
                                                             lat_lon_t distance, 
                                                             vertex_id_t pred_id)
       {
           min_queue_bkwd_.insert(distance, id);
           astar_vertex_data_t& data = vertex_data_bkwd_[id];
           data.color_ = astar_vertex_data_t::VISITED;
           data.distance_ = distance;
           data.predecessor_ = pred_id;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::set_processed_fwd(vertex_id_t id)
       {
           if (mode == debug)
               debug_info_->closed_list1_put_calls_ += 1;
           vertex_data_fwd_[id].color_ = astar_vertex_data_t::PROCESSED;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::set_processed_bkwd(vertex_id_t id)
       {
           vertex_data_bkwd_[id].color_ = astar_vertex_data_t::PROCESSED;
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_in_min_queue_fwd(vertex_id_t id)
       {
           if (mode == debug)
               debug_info_->min_queue_fwd_isin_calls_ += 1;
           return min_queue_fwd_.has_key(id);
           //distance_map_t::iterator it1 = min_queue_fwd_.find(id);
           //if (it1 != min_queue_fwd_.end())
           //{
           //    return true;
           //}
           //else return false;
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_in_min_queue_bkwd(vertex_id_t id)
       {
           return min_queue_bkwd_.has_key(id);
           /*distance_map_t::iterator it1 = min_queue_bkwd_.find(id);
           if (it1 != min_queue_bkwd_.end())
           {
               return true;
           }
           else return false;*/
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_processed_fwd(vertex_id_t id)
       {
           if (mode == debug)
               debug_info_->closed_list1_isin_calls_ += 1;
           bool b1 = vertex_data_fwd_.has_key(id);
           if (!b1) return false;
           
           bool b2 = vertex_data_fwd_.get_value(id).color_ == astar_vertex_data_t::PROCESSED;
           return b1 && b2;
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_processed_bkwd(vertex_id_t id)
       {
           bool b1 = vertex_data_bkwd_.has_key(id);
           if (!b1) return false;
           
           bool b2 = vertex_data_bkwd_.get_value(id).color_ == astar_vertex_data_t::PROCESSED;
           return b1 && b2;
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_min_queue_empty_fwd()
       {
           return min_queue_fwd_.empty();
       }
       template<mode_e mode, directional_e directional>
       bool astar_search_t<mode, directional>::is_min_queue_empty_bkwd()
       {
           return min_queue_bkwd_.empty();
       }
       template<mode_e mode, directional_e directional>
       lat_lon_t astar_search_t<mode, directional>::get_distance_fwd(vertex_id_t id)
       {
           if (mode == debug)
              debug_info_->get_distance_fwd_calls_ += 1;
           return vertex_data_fwd_[id].distance_;
       }
       template<mode_e mode, directional_e directional>
       lat_lon_t astar_search_t<mode, directional>::get_distance_bkwd(vertex_id_t id)
       {
           return vertex_data_bkwd_[id].distance_;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::update_vertex_fwd(vertex_id_t id, lat_lon_t dist, vertex_id_t id_pred)
       {
           if (mode == debug)
              debug_info_->set_predecessor_fwd_calls_ += 1;
           astar_vertex_data_t& data = vertex_data_fwd_[id];
           data.distance_ = dist;
           data.predecessor_ = id_pred;
           min_queue_fwd_.dec_key_by_id(dist, id);
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::update_vertex_bkwd(vertex_id_t id, lat_lon_t dist, vertex_id_t id_pred)
       {
           astar_vertex_data_t& data = vertex_data_bkwd_[id];
           data.distance_ = dist;
           data.predecessor_ = id_pred;
           min_queue_bkwd_.dec_key_by_id(dist, id);
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::collect_stats_fwd()
       {
            //collecting statistics
            debug_info_->num_iterates_ += 1;
            vertex_t& current_v = view_->get_vertex_by_id(current_v_id_fwd_); 
            vertex_t& prev_v = view_->get_vertex_by_id(debug_info_->deb_vertex_prev1_.id_); 

            debug_info_->deb_vertex_current1_.distance_ = get_distance_fwd(current_v_id_fwd_);            
            debug_info_->deb_vertex_current1_.heuristic_ = math::distance(current_v, finish_p_);
            debug_info_->deb_vertex_current1_.lat_ = current_v.get_point().lat_;
            debug_info_->deb_vertex_current1_.lon_ = current_v.get_point().lon_;
            debug_info_->deb_vertex_current1_.id_ = current_v_id_fwd_;

            debug_info_->deb_vertex_prev1_.distance_ = get_distance_fwd(debug_info_->deb_vertex_prev1_.id_);
            debug_info_->deb_vertex_prev1_.heuristic_ = math::distance(prev_v, finish_p_);
            debug_info_->deb_vertex_prev1_.lat_ = prev_v.get_point().lat_;
            debug_info_->deb_vertex_prev1_.lon_ = prev_v.get_point().lon_;
            //adding adjacent vertices, which are not processed yet
            debug_info_->deb_vertex_prev1_adj.erase(debug_info_->deb_vertex_prev1_adj.begin(), 
                                                    debug_info_->deb_vertex_prev1_adj.end());
            typename view_adj_vertices_iterator_pair_t<graph::forward_adj>::type 
               prev_adj_v_it_pair = adj_vertices<graph::forward_adj>(debug_info_->deb_vertex_prev1_.id_, *view_);
            for(; prev_adj_v_it_pair.first != prev_adj_v_it_pair.second; ++prev_adj_v_it_pair.first)
            {
                if (!is_processed_fwd( (*prev_adj_v_it_pair.first).get_id() ))
                {
                    vertex_debug_t v_d;
                    v_d.id_ = (*prev_adj_v_it_pair.first).get_id();
                    v_d.distance_ = get_distance_fwd((*prev_adj_v_it_pair.first).get_id());
                    v_d.heuristic_ = math::distance((*prev_adj_v_it_pair.first), finish_p_ );
                    v_d.lat_ = (*prev_adj_v_it_pair.first).get_point().lat_;
                    v_d.lon_ = (*prev_adj_v_it_pair.first).get_point().lon_;
                    debug_info_->deb_vertex_prev1_adj.push_back(v_d);
                }
            }
            //end adding adj vertices
            debug_info_->deb_vertex_prev1_.id_ = current_v_id_fwd_;
            //end collecting statistics
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::process_current_adjacent_fwd()
       {
            typename view_adj_vertices_iterator_pair_t<graph::forward_adj>::type 
               it = adj_vertices<graph::forward_adj>(current_v_id_fwd_, *view_);
            for(;it.first != it.second; ++it.first)
            {
                if (mode == debug)
                  debug_info_->visited_edges_ += 1;
                if (!is_processed_fwd( (*it.first).get_id() ))
                {                    
                    vertex_t& current_v = view_->get_vertex_by_id(current_v_id_fwd_);                   
                    if (!is_in_min_queue_fwd( (*it.first).get_id() ))
                    {
                        lat_lon_t new_dist = get_distance_fwd(current_v_id_fwd_) + it.first.edge().get_length();
                        //changing length of the edge by potential function
                        new_dist += -get_potential<forward>(current_v.get_point()) + get_potential<forward>((*it.first).get_point());
                        new_vertex_fwd((*it.first).get_id(), new_dist, current_v_id_fwd_);

                        if (mode == debug)
                        {
                           debug_info_->visited_vertices_ += 1;
                           //drawing
                           debug_info_->output_list_fwd_.push_back(line_t(
                               point_t(current_v.get_point().lat_,current_v.get_point().lon_),
                               point_t((*it.first).get_point().lat_,(*it.first).get_point().lon_))
                               ); 
                        }
                    }
                    else
                    {
                        lat_lon_t new_dist = get_distance_fwd(current_v_id_fwd_) + it.first.edge().get_length();
                        //changing length of the edge by potential function
                        new_dist += -get_potential<forward>(current_v.get_point()) + get_potential<forward>((*it.first).get_point());
                        if (get_distance_fwd((*it.first).get_id()) > new_dist)
                            update_vertex_fwd((*it.first).get_id(), new_dist, current_v_id_fwd_);
                    }
                    
                    //checking exit conditions for symmentric approach
                    if (directional == bidirectional)
                    {
                        if (is_in_min_queue_bkwd( (*it.first).get_id() ) || is_processed_bkwd((*it.first).get_id()))
                        {
                            vertex_t& current_v = view_->get_vertex_by_id(current_v_id_fwd_);
                            vertex_t& next_v = view_->get_vertex_by_id((*it.first).get_id());
                            
                            lat_lon_t path_len = 0;
                            lat_lon_t temp = get_distance_fwd(current_v.get_id()) - potential_fwd(current_v.get_point());
                            path_len+= temp;
                            temp = get_distance_bkwd(next_v.get_id()) - potential_bkwd(next_v.get_point()) ;
                            path_len+= temp;
                            temp = math::distance(current_v.get_point().lat_, current_v.get_point().lon_, 
                                                  next_v.get_point().lat_, next_v.get_point().lon_);
                            path_len+= temp;

                            if (path_len < min_path_len_of_bidirect_search_)
                            {
                                min_path_len_of_bidirect_search_ = path_len; 
                                sh_path_vert_fwd_ = current_v_id_fwd_;
                                sh_path_vert_bkwd_ = next_v.get_id();
                            }
                        }                         
                    }
                }
            }
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::run_undirect(bool iterate, unsigned long long iteration_number)
       {     
           if (mode == debug)
           {
               bool old_iterate = iterate;
               iterate = true;
               while( 
                   (current_v_id_fwd_ != finish_vertex_id_) && 
                   (!is_min_queue_empty_fwd()) && 
                   iterate && 
                   (debug_info_->num_iterates_ < iteration_number || iteration_number == 0))
               {
                   if (!old_iterate) iterate = false;
                   current_v_id_fwd_ = extract_min_fwd();
                   set_processed_fwd(current_v_id_fwd_);
                   collect_stats_fwd(); 
                   process_current_adjacent_fwd();
               }
           } 
           else if (mode == release)
           {
               while( 
                    (current_v_id_fwd_ != finish_vertex_id_) && 
                    (!is_min_queue_empty_fwd()))
               {
                   current_v_id_fwd_ = extract_min_fwd();
                   set_processed_fwd(current_v_id_fwd_);
                   process_current_adjacent_fwd();
               }
           }
           if (current_v_id_fwd_ == finish_vertex_id_) 
               is_ended_fwd_ = true;
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::collect_stats_bkwd()
       {
           //collecting statistics
           debug_info_->num_iterates2_ += 1;
           vertex_t& current_v2 = view_->get_vertex_by_id(current_v_id_bkwd_); 
           vertex_t& prev_v2 = view_->get_vertex_by_id(debug_info_->deb_vertex_prev2_.id_); 

           debug_info_->deb_vertex_current2_.distance_ = get_distance_bkwd(current_v_id_bkwd_);            
           debug_info_->deb_vertex_current2_.heuristic_ = math::distance(current_v2, start_p_ );
           debug_info_->deb_vertex_current2_.lat_ = current_v2.get_point().lat_;
           debug_info_->deb_vertex_current2_.lon_ = current_v2.get_point().lon_;
           debug_info_->deb_vertex_current2_.id_ = current_v_id_bkwd_;

           debug_info_->deb_vertex_prev2_.distance_ = get_distance_bkwd(debug_info_->deb_vertex_prev2_.id_);
           debug_info_->deb_vertex_prev2_.heuristic_ = math::distance(prev_v2, start_p_ );
           debug_info_->deb_vertex_prev2_.lat_ = prev_v2.get_point().lat_;
           debug_info_->deb_vertex_prev2_.lon_ = prev_v2.get_point().lon_;
           //adding adjacent vertices, which are not processed yet
           debug_info_->deb_vertex_prev2_adj.erase(debug_info_->deb_vertex_prev2_adj.begin(), 
                                                   debug_info_->deb_vertex_prev2_adj.end());
           typename view_adj_vertices_iterator_pair_t<graph::reverse_adj>::type
               prev_adj_v_it_pair2 = adj_vertices<graph::reverse_adj>(debug_info_->deb_vertex_prev2_.id_, *view_);
           for(; prev_adj_v_it_pair2.first != prev_adj_v_it_pair2.second; ++prev_adj_v_it_pair2.first)
           {
               if (!is_processed_bkwd( (*prev_adj_v_it_pair2.first).get_id() ))
               {
                   vertex_debug_t v_d;
                   v_d.id_ = (*prev_adj_v_it_pair2.first).get_id();
                   v_d.distance_ = get_distance_bkwd((*prev_adj_v_it_pair2.first).get_id());
                   v_d.heuristic_ = math::distance((*prev_adj_v_it_pair2.first), start_p_ );
                   v_d.lat_ = (*prev_adj_v_it_pair2.first).get_point().lat_;
                   v_d.lon_ = (*prev_adj_v_it_pair2.first).get_point().lon_;
                   debug_info_->deb_vertex_prev2_adj.push_back(v_d);
               }
           }
           //end adding adj vertices
           debug_info_->deb_vertex_prev2_.id_ = current_v_id_bkwd_;
           //end collecting statistics

       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::process_current_adjacent_bkwd()
       {                                                             
           view_adj_vertices_iterator_pair_t<graph::reverse_adj>::type 
               it2 = adj_vertices<graph::reverse_adj>(current_v_id_bkwd_, *view_);

           for(; it2.first != it2.second; ++it2.first)
           {
               if (mode == debug)
                  debug_info_->visited_edges2_ += 1;
               if (!is_processed_bkwd( (*it2.first).get_id() ))
               {                        
                   vertex_t& current_v2 = view_->get_vertex_by_id(current_v_id_bkwd_);                   
                   if (!is_in_min_queue_bkwd( (*it2.first).get_id() ))
                   {
                       lat_lon_t new_dist = get_distance_bkwd(current_v_id_bkwd_) + it2.first.edge().get_length();
                       //changing length of the edge by potential function
                       new_dist += -get_potential<backward>(current_v2.get_point()) + get_potential<backward>((*it2.first).get_point());                       
                       new_vertex_bkwd((*it2.first).get_id(), new_dist, current_v_id_bkwd_);
                       if (mode == debug)
                       {
                          debug_info_->visited_vertices2_ += 1;
                          //drawing
                          debug_info_->output_list_bkwd_.push_back(line_t(
                              point_t(current_v2.get_point().lat_,current_v2.get_point().lon_),
                              point_t((*it2.first).get_point().lat_,(*it2.first).get_point().lon_))
                              );
                      }
                   }
                   else
                   {
                       lat_lon_t new_dist = get_distance_bkwd(current_v_id_bkwd_) + it2.first.edge().get_length();
                       //changing length of the edge by potential function
                       new_dist += -get_potential<backward>(current_v2.get_point()) + get_potential<backward>((*it2.first).get_point());
                       if (get_distance_bkwd((*it2.first).get_id()) > new_dist)
                           update_vertex_bkwd((*it2.first).get_id(), new_dist, current_v_id_bkwd_);
                   }        
                   //check exit condition for bidirectional search
                   if (directional == bidirectional)
                   {
                       if (is_in_min_queue_fwd( (*it2.first).get_id() ) || is_processed_fwd((*it2.first).get_id()))
                       {
                           vertex_t& current_v2 = view_->get_vertex_by_id(current_v_id_bkwd_);
                           vertex_t& next_v = view_->get_vertex_by_id((*it2.first).get_id());
                           
                           lat_lon_t path_len = 0;
                           lat_lon_t temp = get_distance_bkwd(current_v2.get_id()) - potential_bkwd(current_v2.get_point());
                           path_len+= temp;
                           temp = get_distance_fwd(next_v.get_id()) - potential_fwd(next_v.get_point());
                           path_len+= temp;
                           temp = math::distance(current_v2.get_point().lat_, current_v2.get_point().lon_, 
                                                 next_v.get_point().lat_, next_v.get_point().lon_);
                           path_len+= temp;

                           if (path_len < min_path_len_of_bidirect_search_)
                           {
                               min_path_len_of_bidirect_search_ = path_len;
                               sh_path_vert_fwd_ = next_v.get_id();
                               sh_path_vert_bkwd_ = current_v_id_bkwd_;
                           }
                       }
                   }
               }

           }
       }
       template<mode_e mode, directional_e directional>
       void astar_search_t<mode, directional>::run_bidirect(bool iterate, unsigned long long iteration_number)
       {          
           do
           {            
               if (!is_ended_fwd_)
               {
                   current_v_id_fwd_ = extract_min_fwd();
                   //std::cout << current_v_id_fwd_ << std::endl;
                   set_processed_fwd(current_v_id_fwd_);
                   if (mode == debug)
                       collect_stats_fwd();
                   if (is_processed_bkwd(current_v_id_fwd_))
                   {
                       is_ended_fwd_ = 1;
                       is_ended_bkwd_ = 1;
                       break;
                   }
                   process_current_adjacent_fwd();
               }
               if (!is_ended_bkwd_)
               {                
                   current_v_id_bkwd_ = extract_min_bkwd();                
                   //std::cout << current_v_id_bkwd_ << std::endl;
                   set_processed_bkwd(current_v_id_bkwd_);
                   if (mode == debug)
                       collect_stats_bkwd();
                   if (is_processed_fwd(current_v_id_bkwd_))
                   {
                       is_ended_fwd_ = 1;
                       is_ended_bkwd_ = 1;
                       break;
                   }
                   process_current_adjacent_bkwd();                
               }
           }
           while( 
               (!is_ended_fwd_ || !is_ended_bkwd_) && 
               (!is_min_queue_empty_fwd()) && 
               (!is_min_queue_empty_bkwd()) &&
               ( mode == release ||
               (iterate && (
                (debug_info_->num_iterates_ < iteration_number && debug_info_->num_iterates2_ < iteration_number) || 
                iteration_number == 0)
               )
               )
               );
       }

      template<mode_e mode, directional_e directional>
      inline void astar_search_t<mode, directional>::fill_shpath(std::list<lat_lon_t>& list) const
      {
          if (is_all_ended())
          {
             //undirectional - start vertex is in the back of result list
             //bidirectional - start vertex is in the back of result list
             if (directional == undirectional)
             {
                 vertex_id_t temp_id = finish_vertex_id_;
                 while (temp_id != start_vertex_id_)
                 {
                     vertex_t const & temp_v = get_vertex_by_id(temp_id);
                     list.push_back(temp_v.get_point().lat_);
                     list.push_back(temp_v.get_point().lon_);       
                     temp_id = vertex_data_fwd_.get_value(temp_id).predecessor_;
                 }
                 //adding start vertex
                 list.push_back(start_p_.lat_);
                 list.push_back(start_p_.lon_);
             }
             else if (directional == bidirectional)
             {
                 vertex_id_t temp_id = sh_path_vert_fwd_;
                 vertex_id_t prev_temp_id;
                 while (temp_id != start_vertex_id_)
                 {
                    //std::cout << temp_id  << std::endl;
                    vertex_t const & temp_v = get_vertex_by_id(temp_id);
                    list.push_back(temp_v.get_point().lat_);
                    list.push_back(temp_v.get_point().lon_);
                    prev_temp_id = temp_id;
                    temp_id = vertex_data_fwd_.get_value(temp_id).predecessor_;
                 }
                 //adding start vertex
                 if (start_vertex_id_ != START_VERTEX_ID)
                 {
                    list.push_back(start_p_.lat_);
                    list.push_back(start_p_.lon_);
                 }

                 if (nearest_start_ps_.has_key(prev_temp_id))
                 {
                    point_t p = nearest_start_ps_.get_value(prev_temp_id);
                    list.push_back(p.lat_);
                    list.push_back(p.lon_);
                 }

                 temp_id = sh_path_vert_bkwd_;
                 while (temp_id != finish_vertex_id_)
                 {
                    vertex_t const & temp_v = get_vertex_by_id(temp_id);
                    list.push_front(temp_v.get_point().lon_); //push_front?
                    list.push_front(temp_v.get_point().lat_);   
                    prev_temp_id = temp_id;
                    temp_id = vertex_data_bkwd_.get_value(temp_id).predecessor_;
                 }
                 //adding finish vertex
                 if (finish_vertex_id_ != FINISH_VERTEX_ID)
                 {
                    list.push_front(finish_p_.lon_);
                    list.push_front(finish_p_.lat_);
                 }

                 if (nearest_finish_ps_.has_key(prev_temp_id))
                 {
                    point_t p = nearest_finish_ps_.get_value(prev_temp_id);
                    list.push_front(p.lon_);
                    list.push_front(p.lat_);
                 }
             }      
          }
      }
      template<mode_e mode, directional_e directional>
      inline vertex_id_t astar_search_t<mode, directional>::get_cur_vertex_id_fwd() const
      {
         return current_v_id_fwd_;
      }
      template<mode_e mode, directional_e directional>
      inline vertex_id_t astar_search_t<mode, directional>::get_cur_vertex_id_bkwd() const
      {
         return current_v_id_bkwd_;
      }
      template<mode_e mode, directional_e directional>
      inline lat_lon_t astar_search_t<mode, directional>::get_start_lat() const
      {
         return start_p_.lat_;
      }
      template<mode_e mode, directional_e directional>
      inline lat_lon_t astar_search_t<mode, directional>::get_start_lon() const
      {
         return start_p_.lon_;
      }
      template<mode_e mode, directional_e directional>
      inline lat_lon_t astar_search_t<mode, directional>::get_finish_lat() const
      {
         return finish_p_.lat_;
      }  
      template<mode_e mode, directional_e directional>
      inline lat_lon_t astar_search_t<mode, directional>::get_finish_lon() const
      {
         return finish_p_.lon_;
      }
      
      template<mode_e mode, directional_e directional>
      inline vertex_id_t astar_search_t<mode, directional>::get_start_vertex_id() const
      {
         return start_vertex_id_;
      }
      template<mode_e mode, directional_e directional>
      inline vertex_id_t astar_search_t<mode, directional>::get_finish_vertex_id() const
      {
         return finish_vertex_id_;
      }
   }
}
